isr_stack go_v86(u16int cs, u16int ip, general_registers input) {
	iret_image jump_data;
	jump_data.eip = ip;
	jump_data.cs = cs;
	jump_data.eflags = 0x20202;
	jump_data.esp = 0xFFF0;
	jump_data.ss = 0x8000;
	jump_data.es = cs;
	jump_data.ds = cs;
	jump_data.fs = cs;
	jump_data.gs = cs;

	make_v86_jump(input, jump_data);
	return v86_return;
}

void store_kernel_esp(u32int location) {
	kernel_esp = location;
	sole_tss_entry.esp = location;
}

void stop_signal(isr_stack* state) {
	start_interrupts();
	v86_return = *state;
	end_v86_jump(kernel_esp);
}

void copy_16bit_code(void* code_start, void* code_end, u16int segment) {
	int size = (u8int*)code_end - (u8int*)code_start;
	int destination = segment * 0x10;
	//printf("Copying 16 Bit Code To Region: [%p - %p]\n", destination, destination+size);
	memcpy((u8int*)destination, (u8int*)code_start, size);
}

bool v86_monitor(isr_stack* state) {
	int prefixes = 0;
	int instruction = get_instruction(state, &prefixes);
	
	#ifdef DEBUG_V86_OPCODES
	printf("%x.%x ", prefixes, instruction);
	#endif
	
	switch(instruction) {
		case 0x9C:
			return v86_handle_pushf(state, prefixes);
		case 0x9D:
			return v86_handle_popf(state, prefixes);
		case 0xCD:
			if(*((u8int*)to_linear(state->cs, state->eip)) == 0x3)
				stop_signal(state);
			return v86_handle_intn(state);
		case 0xCF:
			return v86_handle_iret(state, prefixes);
		case 0xFA:
			return v86_handle_cli(state);
		case 0xFB:
			return v86_handle_sti(state);
		case 0xEE:
			return v86_handle_outdxal(state);
		case 0xEF:
			return v86_handle_outdxax(state);
		case 0xEC:
			return v86_handle_indxal(state);
		case 0xED:
			return v86_handle_indxax(state);
		default:
			printf("\nVM86: Unknown instruction! Opcode: 0x%x\n", instruction);
			return FALSE;
	}
}

u8int get_instruction(isr_stack* state, u32int* prefixes) {
	*prefixes = 0;
	u8int instruction = 0;
	
	while(TRUE) {
		instruction = v86_fetchip(state);
		printf(".");
		switch(instruction) {
			case 0x26: *prefixes |= PFX_ES; break;
			case 0x2E: *prefixes |= PFX_CS; break;
			case 0x36: *prefixes |= PFX_SS; break;
			case 0x3E: *prefixes |= PFX_DS; break;
			case 0x64: *prefixes |= PFX_FS; break;
			case 0x65: *prefixes |= PFX_GS; break;
			case 0x66: *prefixes |= PFX_OP32; break;
			case 0x67: *prefixes |= PFX_ADR32; break;
			case 0xF0: *prefixes |= PFX_LOCK; break;
			case 0xF2: *prefixes |= PFX_REPNE; break;
			case 0xF3: *prefixes |= PFX_REP; break;
			default:
				printf("\b");
				return instruction;
		}
	}
}

bool v86_handle_intn(isr_stack* state) {
	u8int int_num = v86_fetchip(state);
	v86_push16(state, state->eflags);
	v86_push16(state, state->cs);
	v86_push16(state, state->eip);
	state->eflags &= ~0x200;
	int_num *= 4;
	state->eip = (state->eip & ~0xFFFF) | peekw(0, int_num + 0);
	state->cs = peekw(0, int_num + 2);
	return TRUE;
}

bool v86_handle_pushf(isr_stack* state, u32int prefixes) {
	if(prefixes & PFX_OP32)
		v86_push32(state, state->eflags);
	else
		v86_push16(state, state->eflags);
	return TRUE;
}

bool v86_handle_popf(isr_stack* state, u32int prefixes) {
	if(prefixes & PFX_OP32) {
		if(state->v86_sp > 0xFFFC)
			return FALSE;
		state->eflags = v86_pop32(state);
	}
	else {
		if(state->v86_sp > 0xFFFE)
			return FALSE;
		state->eflags = (state->eflags & 0xFFFF0000L) | v86_pop16(state);
	}
	return TRUE;
}

bool v86_handle_iret(isr_stack* state, u32int prefixes) {
	if(prefixes & PFX_OP32) {
		if(state->v86_sp > 0xFFF4)
			return FALSE;
		state->eip = v86_pop32(state);
		state->cs = v86_pop32(state);
		state->eflags = v86_pop32(state);
	}
	else {
		if(state->v86_sp > 0xFFFA)
			return FALSE;
		state->eip = v86_pop16(state);
		state->cs = v86_pop16(state);
		state->eflags = (state->eflags & 0xFFFF0000L) |
			v86_pop16(state);
	}
	return TRUE;
}

bool v86_handle_outdxal(isr_stack* state) {
	u16int port = state->edx;
	u8int value = state->eax;
	outb(port, value);
	return TRUE;
}

bool v86_handle_outdxax(isr_stack* state) {
	u16int port = state->edx;
	u16int value = state->eax;
	outw(port, value);
	return TRUE;
}

bool v86_handle_sti(isr_stack* state) {
	state->eflags |= 0x200;
	return TRUE;
}

bool v86_handle_cli(isr_stack* state) {
	state->eflags &= ~0x200;
	return TRUE;
}  

bool v86_handle_indxal(isr_stack* state) {
	u16int port = state->edx;
	u8int value = inb(port);
	state->eax = (state->eax & 0xFFFFFF00) | value;
	return TRUE;
}

bool v86_handle_indxax(isr_stack* state) {
	u16int port = state->edx;
	u16int value = inw(port);
	state->eax = (state->eax & 0xFFFF0000) | value;
	return TRUE;
}

u32int v86_fetchip(isr_stack* state) {
	u8int byte;
	byte = peekb(state->cs, state->eip);
	state->eip = (state->eip + 1) & 0xFFFF;
	return byte;
}

void v86_push16(isr_stack* state, u32int value) {
	state->v86_sp = (state->v86_sp - 2) & 0xFFFF;
	pokew(state->v86_ss, state->v86_sp, value);
}

u32int v86_pop16(isr_stack* state) {
	u32int rv;
	rv = peekw(state->v86_ss, state->v86_sp);
	state->v86_sp = (state->v86_sp + 2) & 0xFFFF;
	return rv;
}

void v86_push32(isr_stack* state, u32int value) {
	state->v86_sp = (state->v86_sp - 4) & 0xFFFF;
	pokel(state->v86_ss, state->v86_sp, value);
}

u32int v86_pop32(isr_stack* state) {
	u32int rv;
	rv = peekl(state->v86_ss, state->v86_sp);
	state->v86_sp = (state->v86_sp + 4) & 0xFFFF;
	return rv;
}

u32int pair_to_linear(u32int seg, u32int off) {
	return seg * 0x10 + off;
}

u16int peekb(u32int seg, u32int off) {
	return *(u8int*)(pair_to_linear(seg, off));
}

u16int peekw(u32int seg, u32int off) {
	return *(u16int*)(to_linear(seg, off));
}

u32int peekl(u32int seg, u32int off) {
	return *(u32int*)(to_linear(seg, off));
}

void pokeb(u32int seg, u32int off, u8int val) {
	*(u8int*)(to_linear(seg, off)) = val;
}

void pokew(u32int seg, u32int off, u16int val) {
	*(u16int*)(to_linear(seg, off)) = val;
}

void pokel(u32int seg, u32int off, u32int val) {
	*(u32int*)(to_linear(seg, off)) = val;
}
